#include <fstream>
#include <vector>
#include <iostream>
#include <algorithm>
#include <cmath>

#include "func.h"
#include "next_node.h"
#include "perebor.h"
#include "is_eq.h"
#include "int2str.h"
#include "gradient.h"
#include "length.h"
#include "build_upr.h"
#include "find_file.h"
#include "vec_in_file.h"
#include "table.h"
#include "zadacha.h"
#include "struct.h"
#include "hessian.h"
#include "inv.h"
#include "print.h"

using namespace std;

tfun int grad_method (file_names Fnames, tip_metoda Tmetod, tout struct oresult ores); //модифицированный градиентный метод
double H (valarray<double> grad, valarray<double> w, double h, file_names& Fnames, input& data);
double H_newton (valarray<double> grad, valarray<double> w, double h, file_names& Fnames, input& data, vector<vector<double> > Hess);
valarray<double> newton (valarray<double> w, valarray<double> distr, file_names& Fnames, input& data, ofstream &out);      //метод Ньютона-Рафсона


void Kus_post (unsigned tip_integr, string intxt);           //функция поиска кусочно-постоянного управления
void Kus_lin_nepr (unsigned tip_integr, string intxt);       //функция поиска кусочно-линейного непрерывного управления
void Kus_lin_razr (unsigned tip_integr, string intxt);       //функция поиска кусочно-линейного разрывного управления


tfun int main (int argc, char *argv[]) {
    string intxt = "params_movable.txt";                                   //главный входной файл с параметрами исследуемой задачи
    input data;
    string infile(intxt);
    data.init(infile);
    unsigned tip = data.tip;
    unsigned tip_integr = data.integr;
    string files_x = "file_names_optim_x.txt";
    string files_u = "file_names_optim_u.txt";
    std::remove(files_x.c_str());
    std::remove(files_u.c_str());

    switch(tip) {
        case 1:
	  Kus_post(tip_integr,intxt);
          break;
        case 2:
	  Kus_lin_nepr(tip_integr,intxt);
          break;
        case 3:
	  Kus_lin_razr(tip_integr,intxt);
          break;
        case 4:
	  Kus_post(tip_integr,intxt);
	  Kus_lin_nepr(tip_integr,intxt);
          break;
        case 5:
	  Kus_post(tip_integr,intxt);
	  Kus_lin_razr(tip_integr,intxt);
          break;
        case 6:
	  Kus_lin_nepr(tip_integr,intxt);
	  Kus_lin_razr(tip_integr,intxt);
          break;
        case 7:
	  Kus_post(tip_integr,intxt);
	  Kus_lin_nepr(tip_integr,intxt);
	  Kus_lin_razr(tip_integr,intxt);
          break;
    }
    return 0;
}

void Kus_post (unsigned tip_integr, string intxt) {
    string file_mins = "kus_post_minimums.txt";              //хранит пары "значение минимума -- подобласть" (для поиска названия файла с минимумом)
    remove(file_mins.c_str());

    input data;
    const unsigned kp = 1;                                   //управление кусочно-постоянное
    data.set_method(kp, tip_integr);
    string infile(intxt);
    data.init(infile);

    unsigned max_perekl = 11;
    if (data.sum_c > max_perekl) {
        cout << "Too much switch points!" << endl;
        return;
    }

    unsigned n = data.n;                                     //размерность минимизируемой функции
    valarray<double> w(n), p(n);                             //нач. прибл.; обл. поиска (область изменения управления); число разбиений по каждой переменной
    vector<double> rgn(2*n);
    rgn = data.rgn;                                          //область поиска минимума

    unsigned delim = data.delim;
    for (unsigned i=0; i<n; i++) p[i]=delim;
    valarray<double> step(n), start(n), stop(n);
    for (unsigned i=0; i<n; i++) {
        step[i] = abs(rgn[2*i+1]-rgn[2*i])/p[i];
        start[i] = rgn[2*i]+step[i]/2.0;
        stop[i] = rgn[2*i+1]-step[i]/2.0;
    }

    unsigned nodes = 1;
    for (unsigned i=0; i<n; i++) nodes *= p[i];

    string file_name_in = "kus_post_in";                     //строки, формирующие названия входных и выходных файлов
    string file_name_out = "kus_post_out";
    string tr_x = "traject_";
    string upr_traject = "upravl_";

    tip_metoda Tmetod[nodes];
    file_names Fnames[nodes];
    w = start;
    for (unsigned k=0; k<nodes; k++) {
        string s_in(file_name_in + Int2Str(k) + ".txt");     //строим названия входных файлов для функции поиска минимума
        string s_out(file_name_out + Int2Str(k) + ".txt");   //строим названия выходных файлов для функции поиска минимума
        string traject(tr_x + file_name_out + Int2Str(k) + ".txt");
        strcpy(Fnames[k].in_file, s_in.c_str());
        strcpy(Fnames[k].out_file, s_out.c_str());
        strcpy(Fnames[k].runge, traject.c_str());
        strcpy(Fnames[k].main_input, intxt.c_str());
        strcpy(Fnames[k].mins, file_mins.c_str());
        Tmetod[k].tip_upr = kp;
        Tmetod[k].tip_integr = tip_integr;
        ofstream out(s_in.c_str(), ios::trunc);
        for (unsigned i=0; i<n; i++) out << w[i] << " ";
        out << endl;
        for (unsigned i=0; i<n; i++) {
            out << w[i]-step[i]/2 << " "
                << w[i]+step[i]/2 << endl;
        }
        out << k << endl;
        out.close();
        w = next_node(w, start, stop, step);
    }
    const unsigned kol = nodes;
    tval struct oresult ores[kol];
    

    for (unsigned i=0; i < nodes; i++) {                                                                      
        tct(atRank(i % ts::realsuperSize));
        grad_method(Fnames[i], Tmetod[i], ores[i]);
    }


    for (unsigned i=0; i < nodes; i++) {
        write_vector(Fnames[i].mins, ores[i]); 
        struct oresult res = (struct oresult) ores[i];
    }

    vector<string> file_w =  find_file(file_mins, file_name_out, nodes);

    for (unsigned i=0; i<file_w.size(); i++) {
        cout << "Optimum in file " << file_w[i] << endl;
    }

    build_upr(file_w, upr_traject, kp, data);

    string file_optim_x = "file_names_optim_x.txt";
    string file_optim_u = "file_names_optim_u.txt";
    ofstream files_x(file_optim_x.c_str(), ios::app);
    ofstream files_u(file_optim_u.c_str(), ios::app);


    for (unsigned i=0; i<file_w.size(); i++) {
        string file_upr = (upr_traject+file_w[i]);
        cout << "Control in file " << file_upr << endl;
        files_u << file_upr << endl;
        string file_x = (tr_x+file_w[i]);
        cout << "States in file " << file_x << endl;
        files_x << file_x << endl;
    }
    cout << endl << endl;
    files_u.close();
    files_x.close();
}


void Kus_lin_nepr (unsigned tip_integr, string intxt) {
    string file_mins = "kus_lin_nepr_minimums.txt";          //хранит пары "значение минимума -- подобласть" (для поиска названия файла с минимумом)
    remove(file_mins.c_str());

    input data;
    const unsigned kln = 2;                                  //управление кусочно-линейное непрерывное
    data.set_method(kln, tip_integr);
    string infile(intxt);
    data.init(infile);

    unsigned max_perekl = 8;
    if (data.sum_c > max_perekl) {
        cout << "Too much switch points!" << endl;
        return;
    }

    unsigned n = data.n;                                     //размерность минимизируемой функции

    valarray<double> w(n), p(n);                             //нач. прибл.; обл. поиска (область изменения управления); число разбиений по каждой переменной
    vector<double> rgn(2*n);
    rgn = data.rgn;                                          //область поиска минимума

    unsigned delim = data.delim;
    for (unsigned i=0; i<n; i++) p[i]=delim;

    valarray<double> step(n), start(n), stop(n);
    for (unsigned i=0; i<n; i++) {
        step[i] = abs(rgn[2*i+1]-rgn[2*i])/p[i];
        start[i] = rgn[2*i]+step[i]/2.0;
        stop[i] = rgn[2*i+1]-step[i]/2.0;
    }

    unsigned nodes = 1;
    for (unsigned i=0; i<n; i++) nodes *= p[i];

    string file_name_in = "kus_lin_nepr_in";                 //строки, формирующие названия входных и выходных файлов
    string file_name_out = "kus_lin_nepr_out";
    string tr_x = "traject_";
    string upr_traject = "upravl_";

    tip_metoda Tmetod[nodes];
    file_names Fnames[nodes];
    w = start;
    for (unsigned k=0; k<nodes; k++) {
        string s_in(file_name_in + Int2Str(k) + ".txt");     //строим названия входных файлов для функции поиска минимума
        string s_out(file_name_out + Int2Str(k) + ".txt");   //строим названия выходных файлов для функции поиска минимума
        string traject(tr_x + file_name_out + Int2Str(k) + ".txt");
        strcpy(Fnames[k].in_file, s_in.c_str());
        strcpy(Fnames[k].out_file, s_out.c_str());
        strcpy(Fnames[k].runge, traject.c_str());
        strcpy(Fnames[k].main_input, intxt.c_str());
        strcpy(Fnames[k].mins, file_mins.c_str());
        Tmetod[k].tip_upr = kln;
        Tmetod[k].tip_integr = tip_integr;
        ofstream out(s_in.c_str(), ios::trunc);
        for (unsigned i=0; i<n; i++) out << w[i] << " ";
        out << endl;
        for (unsigned i=0; i<n; i++) {
            out << w[i]-step[i]/2 << " "
                << w[i]+step[i]/2 << endl;
        }
        out << k << endl;
        out.close();
        w = next_node(w, start, stop, step);
    }
    const unsigned kol = nodes;
    tval struct oresult ores[kol];

    for (unsigned i=0; i < nodes; i++) {
        tct(atRank(i % ts::realsuperSize));
        grad_method(Fnames[i], Tmetod[i], ores[i]);
    }

    for (unsigned i=0; i < nodes; i++) {
      write_vector(Fnames[i].mins, ores[i]);
      struct oresult res = (struct oresult) ores[i];
    }

    vector<string> file_w =  find_file(file_mins, file_name_out, nodes);
    for (unsigned i=0; i<file_w.size(); i++) {
        cout << "Optimum in file " << file_w[i] << endl;
    }

    build_upr(file_w, upr_traject, kln, data);

    string file_optim_x = "file_names_optim_x.txt";
    string file_optim_u = "file_names_optim_u.txt";
    ofstream files_x(file_optim_x.c_str(), ios::app);
    ofstream files_u(file_optim_u.c_str(), ios::app);

    for (unsigned i=0; i<file_w.size(); i++) {
        string file_upr = (upr_traject+file_w[i]);
        cout << "Control in file " << file_upr << endl;
        files_u << file_upr << endl;
        string file_x = (tr_x+file_w[i]);
        cout << "States in file " << file_x << endl;
        files_x << file_x << endl;
    }
    cout << endl << endl;
    files_u.close();
    files_x.close();
}


void Kus_lin_razr (unsigned tip_integr, string intxt) {
    string file_mins = "kus_lin_razr_minimums.txt";          //хранит пары "значение минимума -- подобласть" (для поиска названия файла с минимумом)
    remove(file_mins.c_str());

    input data;
    const unsigned klr = 3;                                  //управление кусочно-линейное разрывное
    data.set_method(klr, tip_integr);
    string infile(intxt);
    data.init(infile);

    unsigned n = data.n;                                     //размерность минимизируемой функции
    valarray<double> w(n), p(n);                             //нач. прибл.; обл. поиска (область изменения управления); число разбиений по каждой переменной
    vector<double> rgn(2*n);
    rgn = data.rgn;                                          //область поиска минимума

    unsigned delim = data.delim;
    for (unsigned i=0; i<n; i++) p[i]=delim;


    valarray<double> step(n), start(n), stop(n);
    for (unsigned i=0; i<n; i++) {
        step[i] = abs(rgn[2*i+1]-rgn[2*i])/p[i];
        start[i] = rgn[2*i]+step[i]/2.0;
        stop[i] = rgn[2*i+1]-step[i]/2.0;
    }

    unsigned nodes = 1;
    for (unsigned i=0; i<n; i++) nodes *= p[i];

    string file_name_in = "kus_lin_razr_in";                 //строки, формирующие названия входных и выходных файлов
    string file_name_out = "kus_lin_razr_out";
    string tr_x = "traject_";
    string upr_traject = "upravl_";

    tip_metoda Tmetod[nodes];
    file_names Fnames[nodes];
    w = start;
    for (unsigned k=0; k<nodes; k++) {
        string s_in(file_name_in + Int2Str(k) + ".txt");     //строим названия входных файлов для функции поиска минимума
        string s_out(file_name_out + Int2Str(k) + ".txt");   //строим названия выходных файлов для функции поиска минимума
        string traject(tr_x + file_name_out + Int2Str(k) + ".txt");
        strcpy(Fnames[k].in_file, s_in.c_str());
        strcpy(Fnames[k].out_file, s_out.c_str());
        strcpy(Fnames[k].runge, traject.c_str());
        strcpy(Fnames[k].main_input, intxt.c_str());
        strcpy(Fnames[k].mins, file_mins.c_str());
        Tmetod[k].tip_upr = klr;
        Tmetod[k].tip_integr = tip_integr;
        ofstream out(s_in.c_str(), ios::trunc);
        for (unsigned i=0; i<n; i++) out << w[i] << " ";
        out << endl;
        for (unsigned i=0; i<n; i++) {
            out << w[i]-step[i]/2 << " "
                << w[i]+step[i]/2 << endl;
        }
        out << k << endl;
        out.close();
        w = next_node(w, start, stop, step);
    }
    const unsigned kol = nodes;
    tval struct oresult ores[kol];


    for (unsigned i=0; i < nodes; i++) {
       tct(atRank(i % ts::realsuperSize));
       grad_method(Fnames[i], Tmetod[i], ores[i]);
    }

    for (unsigned i=0; i < nodes; i++) {
        write_vector(Fnames[i].mins, ores[i]);
        struct oresult res = (struct oresult) ores[i];
    }


    vector<string> file_w =  find_file(file_mins, file_name_out, nodes);
    for (unsigned i=0; i<file_w.size(); i++) {
        cout << "Optimum in file " << file_w[i] << endl;
    }

    build_upr(file_w, upr_traject, klr, data);

    string file_optim_x = "file_names_optim_x.txt";
    string file_optim_u = "file_names_optim_u.txt";
    ofstream files_x(file_optim_x.c_str(), ios::app);
    ofstream files_u(file_optim_u.c_str(), ios::app);


    for (unsigned i=0; i<file_w.size(); i++) {
        string file_upr = (upr_traject+file_w[i]);
        cout << "Control in file " << file_upr << endl;
        files_u << file_upr << endl;
        string file_x = (tr_x+file_w[i]);
        cout << "States in file " << file_x << endl;
        files_x << file_x << endl;
    }
    cout << endl << endl;
    files_u.close();
    files_x.close();
}


//функция вычисления нового шага
double H (valarray<double> grad, valarray<double> w, double h, file_names& Fnames, input& data) {
    unsigned n = w.size();
    valarray<double> norm_grad(n), wn(n), w0(n);
    double dl = Length(grad);
    norm_grad = grad/dl;
    wn = w-h*norm_grad;

    while (func(wn,Fnames,data) > func(w,Fnames,data)) {
        h /= 2;
        wn = w-h*norm_grad;
    }
    const double diff = 1;
    const double delta = 0.01;
    w0=w;

    while (func(wn,Fnames,data) < func(w0,Fnames,data) && (abs(func(w,Fnames,data)-func(wn,Fnames,data))<=diff)) {
        h += delta;
        w0 = wn;
        wn = w-h*norm_grad;
    }
    return (h-delta)/dl;
}



//функция вычисления нового шага (для метода Ньютона)
double H_newton (valarray<double> grad, valarray<double> w, double h, file_names& Fnames, input& data, vector<vector<double> > Hess) {
    unsigned n = w.size();
    vector<vector<double> > inv_h(n, vector<double> (n));  //обратная матрица Гессе
    inv_h = inverse(Hess, n);                              //вычисляем H^{-1}

    valarray<double> prod(n), norm_grad(n);
    double dl=Length(grad);
    norm_grad=grad/dl;

    prod = product_mv(inv_h,norm_grad);                    //H^{-1}*||grad||
    valarray<double> wn(n), w0(n);
    wn = w-h*prod;
    while (func(wn,Fnames,data) > func(w,Fnames,data)) {
        h /= 2;
        wn = w-h*prod;
    }

    const double diff = 1;
    const double delta = 0.01;
    w0 = w;

    while (func(wn,Fnames,data) < func(w0,Fnames,data) && (abs(func(w,Fnames,data)-func(wn,Fnames,data))<=diff)) {
        h += delta;
        w0 = wn;
        wn = w-h*prod;
    }
    return (h-delta)/dl;
}



//модифицированный градиентный метод
tfun int grad_method (file_names Fnames, tip_metoda Tmetod, tout struct oresult ores) {
    struct oresult res;
    input data;
    data.set_method(Tmetod.tip_upr, Tmetod.tip_integr);
    string infile(Fnames.main_input);
    data.init(infile);

    unsigned n = data.n;

//--------------------------- Read input data --------------------------------------------------------------------------------------
    ifstream in(Fnames.in_file);
    valarray<double> w0(n), distr0(2*n);
    for (unsigned i=0; i<n; i++) in >> w0[i];
    for (unsigned i=0; i<2*n; i++) in >> distr0[i];
    int obl;
    in >> obl;
    in.close();
//----------------------------------------------------------------------------------------------------------------------------------

    n = n + data.sum_c;                                      //размерность минимизируемой функции

    valarray<double> w(n), distr(2*n);
    for (unsigned i=0; i<w0.size(); i++) w[i] = w0[i];
    for (unsigned i=0; i<distr0.size(); i++) distr[i] = distr0[i];

    valarray<double> tau0(data.sum_c), rgn_tau(2*data.sum_c);
    tau0 = data.tau0; rgn_tau = data.rgn_tau;
    for (unsigned i=data.n; i<w.size(); i++) w[i] = tau0[i-data.n];
    for (unsigned i=2*data.n; i<distr.size(); i++) distr[i] = rgn_tau[i-2*data.n];

    ofstream output(Fnames.out_file, ios::trunc);            //содержит ход поиска минимума в подобласти
    double (*f)(valarray<double>, file_names& Fnames, input& data) = func;
    valarray<double> ch(n);                                  //число разбиений по каждой переменной

    for (unsigned i=0; i<n; i++) ch[i] = 2;                  //будет сетка 2x2 для n=2
    double delta = 0.1;                                      //размер области перебора
    valarray<double> shag(n);                                //шаг по сетке
    for (unsigned i=0; i<n; i++) shag[i] = delta/ch[i];      //вычисляем шаг
    valarray<bool> comp(n);                                  //содержит результат покомпонентного сравнения векторов
    valarray<double> w_perebor(n);                           //найденная перебором точка минимума
    double hw = 0.01;                                        //на первой итерации шаг принимаем 0.01

    vector<double> fk;

    valarray<double> grad = gradient(w,f,data,Fnames);

    //проверяем, находимся в точке минимума или в стационарности
    if (Length(grad) < data.eps) {
        w_perebor = perebor(w,ch,shag,data,f,Fnames);
        comp = (w==w_perebor);
        if (comp.min() == true) {
            print_res(output, grad, w, hw, Fnames, data);
            output.close();
            res.f = data.F; res.run = obl;
            ores = (struct oresult) res;
            return 1;
        }
        else w = w_perebor;
    }


    double ostanov_ind=0;
    bool ostanov=false, quit_w=false;
    const double max_ostanov=10;
    valarray<double> w_diff(n);
    valarray<bool> stop_w(n);
    valarray<double> w1(n);
    do {
            grad = gradient(w, f, data, Fnames);
            print_res(output, grad, w, hw, Fnames, data);
            w1 = w;
            hw = H(grad,w,hw,Fnames,data);
            w -= grad*hw;

            for (unsigned k=0; k<n; k++) {                   //проверка выхода w за границы
                if ((w[k] < distr[2*k]) || (w[k] > distr[2*k+1])) {
                    ostanov_ind++;
                    if (ostanov_ind > max_ostanov) ostanov=true;
                }
                if (w[k] < distr[2*k]) w[k] = distr[2*k];
                if (w[k] > distr[2*k+1]) w[k] = distr[2*k+1];
            }

            w_diff = w1- w;

            for (unsigned i=0; i<n; i++) {
              if (w_diff[i] <= 1e-3) stop_w[i]=1;
              else stop_w[i]=0;                              //проверка на медленную сходимость
            }

            if (stop_w.min() == 1) quit_w = true;

            for (unsigned i=0; i<n; i++) {
              if (isnan(w[i]) == true) {
                print_res(output, grad, w, 0, Fnames,data);
                output.close();
                res.f = data.F; res.run = obl;
                ores = (struct oresult) res;
                return 1;
              }
            }

        } while ((quit_w!=true) && (ostanov!=true));

    w = newton(w,distr,Fnames,data,output);
    grad = gradient(w,f,data,Fnames);

    print_res(output, grad, w, 0, Fnames,data);
    output.close();
    res.f = data.F; res.run = obl;
    ores = (struct oresult) res;
    return 1;
}


//метод Ньютона-Рафсона
valarray<double> newton (valarray<double> w, valarray<double> distr, file_names& Fnames, input& data, ofstream &out) {
    unsigned n = w.size();
    valarray<double> grad(n), grad_tmp(n), grad_diff(n);
    vector<vector<double> > Hess(n, vector<double> (n));
    vector<vector<double> > inv_h(n, vector<double> (n));
    double hw = 0.01;

    double ostanov_ind=0;
    const double max_ostanov=10;
    bool ostanov=false, quit_grad=false;
    valarray<bool> stop_grad(n);

    do {
        grad = gradient(w, func, data, Fnames);
        print_res(out, grad, w, hw, Fnames, data);
        Hess = hess(w,func,data,Fnames);
        inv_h = inverse(Hess, n);
        hw = H_newton(grad,w,hw,Fnames,data,Hess);

        w -= hw*product_mv(inv_h, grad);

        for (unsigned k=0; k<n; k++) {                       // проверка выхода w за границы
            if ((w[k] < distr[2*k]) || (w[k] > distr[2*k+1])) {
                ostanov_ind++;
                if (ostanov_ind > max_ostanov) ostanov=true;
            }
            if (w[k] < distr[2*k]) w[k] = distr[2*k];
            if (w[k] > distr[2*k+1]) w[k] = distr[2*k+1];
        }

        grad_tmp = grad;
        grad = gradient(w,func,data,Fnames);
        grad_diff = grad_tmp - grad;

        for (unsigned i=0; i<n; i++) {
            if (abs(grad_diff[i]) <= 1e-6) stop_grad[i]=1;
            else stop_grad[i]=0;                             //изменение градиента
        }
        if (stop_grad.min() == 1) quit_grad = true;

    } while ((abs(grad).max() >= data.eps) && (ostanov!=true) && (quit_grad!=true));

    return w;
}
